/*
 ------------------------------------------------------------------------------
 Copyright (C) 2006-2007 Team Blur.

 This file is part of the Quantum Engine source code.

 The Quantum Engine source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The Quantum Engine source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the Quantum Engine source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


#include "gamelib.h"


odQuat		quat_identity(0.0f, 0.0f, 0.0f, 1.0f);


/*
 ==================
 odQuat::SLerp
 ==================
*/
void odQuat::SLerp (const odQuat &from, const odQuat &to, float frac){

	odQuat	tmp;
	float	omega, sinom, cosom;
	float	fScale, tScale;

	if (frac <= 0.0f){
		*this = from;
		return;
	}

	if (frac >= 1.0f){
		*this = to;
		return;
	}

	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;

	if (cosom < 0.0f){
		cosom = -cosom;

		tmp = -to;
	}
	else
		tmp = to;

	if ((1.0f - cosom) > 0.0f){
		omega = M_ACos(cosom);
		sinom = 1.0f / M_Sin(omega);

		fScale = M_Sin((1.0f - frac) * omega) * sinom;
		tScale = M_Sin(frac * omega) * sinom;
	}
	else {
		fScale = 1.0f - frac;
		tScale = frac;
	}

	x = from.x * fScale + tmp.x * tScale;
	y = from.y * fScale + tmp.y * tScale;
	z = from.z * fScale + tmp.z * tScale;
	w = from.w * fScale + tmp.w * tScale;
}

/*
 ==================
 odQuat::ToMat3
 ==================
*/
odMat3 odQuat::ToMat3 (void) const {

	odMat3	mat;

	float	xx, yy, zz;
	float	xy, xz, yz;
	float	wx, wy, wz;
	float	x2, y2, z2;

	x2 = x + x;
	y2 = y + y;
	z2 = z + z;

	xx = x * x2;
	yy = y * y2;
	zz = z * z2;

	xy = x * y2;
	xz = x * z2;
	yz = y * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mat[0].Set(1.0f - (yy + zz), xy - wz, xz + wy);
	mat[1].Set(xy + wz, 1.0f - (xx + zz), yz - wx);
	mat[2].Set(xz - wy, yz + wx, 1.0f - (xx + yy));

	return mat;
}

/*
 ==================
 odQuat::ToString

 This is just a convenience function for printing quaternions
 ==================
*/
const char *odQuat::ToString (int precision) const {

	return Str_FromFloatArray(ToFloatPtr(), 4, precision);
}
